// This sketch interfaces with the Pololu servo controller at:
// http://www.pololu.com/catalog/product/207
// This code is written by Adam (nexisnet) at:
// http://forum.pololu.com/viewtopic.php?f=16&t=745&st=0&sk=t&sd=a&start=15

void motorCommand(byte servo, unsigned int angle){
   //servo is the servo number (typically 0-7)
   //angle is the absolute position from 2000 to 4695?

   //Send a Pololu Protocol command
   Serial.print(0x80,BYTE); //start byte
   Serial.print(0x01,BYTE); //device id
   Serial.print(0x04,BYTE); //command number
   Serial.print(servo,BYTE); //servo number
   //Convert the angle data into two 7-bit bytes
   Serial.print(((angle>>7)&0x3f),BYTE); //data1
   Serial.print((angle&0x7f),BYTE); //data2
}

void motorConfigure(byte servo){
   //Send a Pololu Protocol command
   Serial.print(0x80,BYTE); //start byte
   Serial.print(0x01,BYTE); //device id
   Serial.print(0x01,BYTE); //command number
   Serial.print(servo,BYTE); //servo number
   Serial.print(0x0,BYTE); //instant change
   
   motorCommand(servo, 2000);
}
